from serial.tools import list_ports
import sys
try:
    sys.path.append("../")
except:
    print("error!")
from pydobot.dobot import Dobot
import time

# port = list_ports.comports()[1].device
# for op in list_ports.comports():
#     print(op.device)
port = "/dev/dobot_arm"
device = Dobot(port=port, verbose=False)
device._set_queued_cmd_clear()
device.clear_all_alarms_state()

time.sleep(4)
device.rotate_to(-90.0,0.0,0.0,0.0,True)

positions = [[68.33333333333333, -64.84375], [-40.416666666666664, -20.3125], [-0.4166666666666667, 52.5]]
grab_position_adjust = -27
base_y = -200
for position in positions:
    device.rotate_to(-90.0,0.0,0.0,0.0,True)
    # print(device.pose()[:3])
    # (x, y, z, r, j1, j2, j3, j4) = device.pose()
    device.move_to(0 , -200,0, -90, wait=True)
    grab_x,y = position
    # grab_x = grab_x -(grab_x/100)*grab_x if grab_x >0 else grab_x +(grab_x/100)*grab_x
    grab_y = -y + grab_position_adjust + base_y# limite <100
    # print(grab_x,grab_y)
    if grab_x>100:
        grab_x = 100
    elif grab_x<-100:
        grab_x = -100
    if grab_y>-130:
        grab_y = -130
    elif grab_y<-280:
        grab_y = -280
    if grab_x <0 and grab_y <0 and (grab_x/grab_y<1.2 and grab_x/grab_y>0.8):
        grab_y = -200
        grab_x = 0

    print(grab_x,grab_y)
    grab_z = -31
    z = 0

    device.move_to(-grab_x,grab_y,z,-90,wait=True)
    time.sleep(1)
    # print(device.pose()[:3])
    # device.move_to(grab_x,grab_y,grab_z,r,wait=False)
    device.suck(True)
    device.rotate_to(90.0,0.0,0.0,0.0)
    device.suck(False)
    device.rotate_to(-90.0,0.0,0.0,0.0,True)
    # print(device.pose()[:3])
    # device.move_to(300 , -83, grab_z, r, wait=True)
    # device._set_queued_cmd_clear()
    # device.clear_all_alarms_state()
    # break
